/*
 * Copyright (c) 2024 Black Sesame Technologies
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <type_traits>
#include "dal_msg/dalidl_runtime_cpp/traits.hpp"
#include "dalidl_typesupport_cpp/message_type_support.hpp"
#include "dalidl_typesupport_fastrtps_cpp/message_type_support.h"

#include "fastdds/dds/subscriber/SampleInfo.hpp"
#include "fastdds/dds/core/StackAllocatedSequence.hpp"

#include "fastcdr/Cdr.h"
#include "fastcdr/FastBuffer.h"

using namespace eprosima::fastdds::dds;

template <typename MessageT>
constexpr
	typename std::enable_if_t<
		dalidl_generator_traits::is_message<MessageT>::value,
		const dalidl_message_type_support_t &>
	get_message_type_support_handle()
{
	auto handle = dalidl_typesupport_cpp::get_message_type_support_handle<MessageT>();
	if (!handle)
	{
		throw std::runtime_error("Type support handle unexpectedly nullptr");
	}
	return *handle;
}

class SubListener : public DataReaderListener
{
public:
	SubListener()
		: samples_(0)
	{
	}

	~SubListener() override
	{
	}

	void on_subscription_matched(
		DataReader *,
		const SubscriptionMatchedStatus &info) override
	{
		if (info.current_count_change == 1)
		{
			std::cout << "Subscriber matched." << std::endl;
		}
		else if (info.current_count_change == -1)
		{
			std::cout << "Subscriber unmatched." << std::endl;
		}
		else
		{
			std::cout << info.current_count_change
					  << " is not a valid value for SubscriptionMatchedStatus current count change" << std::endl;
		}
	}

	void on_data_available(
		DataReader *reader) override
	{
		void * ros_message = &mjtype;

		dal_fastrtps_shared_cpp::SerializedData data;
		data.is_cdr_buffer = false;
		data.data = ros_message;
		data.impl = ts_callbacks_;

		eprosima::fastdds::dds::StackAllocatedSequence<void *, 1> data_values;
		const_cast<void **>(data_values.buffer())[0] = &data;
		eprosima::fastdds::dds::SampleInfoSeq info_seq{1};

		if (ReturnCode_t::RETCODE_OK == reader->take(data_values, info_seq, 1)) {
			if (info_seq[0].valid_data) {
				std::cout << "on_data_available: "
						  << "i8: " << (int)mjtype.i8 << ", "
						  << "i16: " << mjtype.i16 << ", "
						  << "str: " << mjtype.str << ", "
						  << std::endl;
			}
		}
	}

	mj_interfaces::msg::Mjtype mjtype;

	std::atomic_int samples_;

	const message_type_support_callbacks_t * ts_callbacks_;

} listener_;